package org.nashua.tt151;

import edu.wpi.first.wpilibj.CANJaguar;
import edu.wpi.first.wpilibj.SimpleRobot;
import edu.wpi.first.wpilibj.can.CANTimeoutException;

public class Shooter extends SimpleRobot {

    Controller.DualAction joy;
    CANJaguar jag1;
    CANJaguar jag2;

    public void robotInit() {
        joy = new Controller.DualAction(1);
        try {
            jag1 = new CANJaguar(1);
            jag2 = new CANJaguar(2);
        } catch (CANTimeoutException ex) {
            ex.printStackTrace();
        }
    }

    public void autonomous() {
    }

    public void operatorControl() {
        while (isOperatorControl() && isEnabled()) {
            joy.queryButtons();
            double volt1 = 0, volt2 = -.5;
            if (joy.getRawButton(Controller.DualAction.Button.A)) {
                volt1 = 5;      //Preset 1
                volt2 = volt1 - .5;
                try {
                    jag1.setX(volt1);
                    jag2.setX(volt2);
                } catch (CANTimeoutException ex) {
                    ex.printStackTrace();
                }
            }
            if (joy.getRawButton(Controller.DualAction.Button.B)) {
                volt1 = 6;      //Preset 2
                volt2 = volt1 - .5;
                try {
                    jag1.setX(volt1);
                    jag2.setX(volt2);
                } catch (CANTimeoutException ex) {
                    ex.printStackTrace();
                }
            }
            if (joy.getRawButton(Controller.DualAction.Button.X)) {
                volt1 = 3;      //Preset 3
                volt2 = volt1 - .5;
                try {
                    jag1.setX(volt1);
                    jag2.setX(volt2);
                } catch (CANTimeoutException ex) {
                    ex.printStackTrace();
                }
            }
            if (joy.getRawButton(Controller.DualAction.Button.Y)) {
                //Insert hopper code here
            }
            if (joy.getRawButton(Controller.DualAction.Button.RIGHT_TRIGGER)) {
                volt1 = Math.min(12, volt1+1);      //Makes sure that maximums are not exceeded
                volt2 = Math.min(11.5, volt2+1);    //Increments by one
                try {
                    jag1.setX(Math.max(0, volt1));  //Makes sure that minimums are not exceeded
                    jag2.setX(Math.max(0, volt2));
                } catch (CANTimeoutException ex) {
                    ex.printStackTrace();
                }
            }
            if (joy.getRawButton(Controller.DualAction.Button.LEFT_TRIGGER)) {
                volt1 = Math.min(12, volt1-1);      //Makes sure that maximums are not exceeded
                volt2 = Math.min(11.5, volt2-1);    //Decrements by one
                try {
                    jag1.setX(Math.max(0, volt1));  //Makes sure that minimums are not exceeded
                    jag2.setX(Math.max(0, volt2));
                } catch (CANTimeoutException ex) {
                    ex.printStackTrace();
                }
            }
            if (joy.getRawButton(Controller.DualAction.Button.RIGHT_BUMPER)) {
                volt1 = Math.min(12, volt1+.1);    //Makes sure that maximums are not exceeded
                volt2 = Math.min(11.5, volt2+.1);  //Increments by .1
                try {
                    jag1.setX(Math.max(0, volt1));  //Makes sure that minimums are not exceeded
                    jag2.setX(Math.max(0, volt2));
                } catch (CANTimeoutException ex) {
                    ex.printStackTrace();
                }
            }
            if (joy.getRawButton(Controller.DualAction.Button.LEFT_BUMPER)) {
                volt1 = Math.min(12, volt1-.1);     //Makes sure that maximums are not exceeded
                volt2 = Math.min(11.5, volt2-.1);   //Decrements by .1
                try {
                    jag1.setX(Math.max(0, volt1));  //Makes sure that minimums are not exceeded
                    jag2.setX(Math.max(0, volt2));
                } catch (CANTimeoutException ex) {
                    ex.printStackTrace();
                }
            }
        }
    }

    public void test() {
    }
}